# -*- encoding: utf-8 -*-
# 测试环境: Python 3.6 版本
import _thread
import json
import socket
import time
import sdk

color_dict = {"yellow": [255, 255, 0],
              "blue": [0, 0, 255],
              "green": [0, 255, 0],
              "red": [255, 0, 0]}


def get_ip():
    ip_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定 IP 广播端口
    ip_sock.bind(('0.0.0.0', 40926))
    return (ip_sock.recvfrom(1024))[1][0]


def main():
    # 路由模式
    address = (get_ip(), int(40923))
    # 直连模式
    # address = ("192.168.2.1", int(40923))
    # 与机器人控制命令端口建立 TCP 连接
    rbs = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    print("Connecting...")
    rbs.connect(address)
    print("Connected!")

    if sdk.send_command(rbs, sdk.command()) == "ok":
        print("init ok")
    if sdk.send_command(rbs, sdk.set_sensor_measure(True)) == "ok":
        print("sensor measure on")
    while True:
        time.sleep(1)
        sdk.send_command(rbs, sdk.set_speed(False, 0, 0, 2))
        print("actual: " + sdk.send_command(rbs, sdk.get_speed()))

    # 关闭端口连接
    rbs.shutdown(socket.SHUT_WR)
    rbs.close()


if __name__ == '__main__':
    main()
